package main

import (
	"container/heap"
	"fmt"
)

// ---------- 基础数据结构 ----------

type Point struct{ X, Y int }

type Grid struct {
	W, H    int
	Walls   map[Point]bool  // 障碍
	Weights map[Point]int   // 进入目标格子的代价，未设置默认1
}

func NewGrid(w, h int) *Grid {
	return &Grid{
		W:       w,
		H:       h,
		Walls:   make(map[Point]bool),
		Weights: make(map[Point]int),
	}
}

func (g *Grid) In(p Point) bool {
	return p.X >= 0 && p.X < g.W && p.Y >= 0 && p.Y < g.H
}
func (g *Grid) Walkable(p Point) bool {
	return g.In(p) && !g.Walls[p]
}
func (g *Grid) CostToEnter(p Point) int {
	if !g.Walkable(p) {
		return 1 << 30
	}
	if c, ok := g.Weights[p]; ok {
		return c
	}
	return 1
}
func (g *Grid) Neighbors4(p Point) []Point {
	n := []Point{
		{p.X, p.Y - 1},
		{p.X, p.Y + 1},
		{p.X - 1, p.Y},
		{p.X + 1, p.Y},
	}
	out := n[:0]
	for _, q := range n {
		if g.Walkable(q) {
			out = append(out, q)
		}
	}
	return out
}

func Manhattan(a, b Point) int {
	dx := a.X - b.X
	if dx < 0 {
		dx = -dx
	}
	dy := a.Y - b.Y
	if dy < 0 {
		dy = -dy
	}
	return dx + dy
}

// reconstruct 从 prev 回溯出路径（不含起点，含终点）
func reconstruct(prev map[Point]Point, start, goal Point) (path []Point, ok bool) {
	if _, exists := prev[goal]; !exists && goal != start {
		return nil, false
	}
	p := goal
	for p != start {
		path = append(path, p)
		p = prev[p]
	}
	// 反转
	for i, j := 0, len(path)-1; i < j; i, j = i+1, j-1 {
		path[i], path[j] = path[j], path[i]
	}
	return path, true
}

// ---------- BFS（无权最短步数） ----------
func BFS(g *Grid, start, goal Point) (path []Point, cost int, ok bool) {
	if !g.Walkable(start) || !g.Walkable(goal) {
		return nil, 0, false
	}
	q := []Point{start}
	seen := map[Point]bool{start: true}
	prev := make(map[Point]Point)
	steps := make(map[Point]int)
	for len(q) > 0 {
		p := q[0]
		q = q[1:]
		if p == goal {
			path, ok = reconstruct(prev, start, goal)
			return path, steps[p], ok
		}
		for _, nb := range g.Neighbors4(p) {
			if !seen[nb] {
				seen[nb] = true
				prev[nb] = p
				steps[nb] = steps[p] + 1
				q = append(q, nb)
			}
		}
	}
	return nil, 0, false
}

// ---------- Dijkstra（非负权；边权=进入邻居格子的代价） ----------
func Dijkstra(g *Grid, start, goal Point) (path []Point, cost int, ok bool) {
	if !g.Walkable(start) || !g.Walkable(goal) {
		return nil, 0, false
	}
	dist := map[Point]int{start: 0}
	prev := make(map[Point]Point)
	pq := new(PQ)
	heap.Push(pq, &node{P: start, Pri: 0})

	visited := make(map[Point]bool)

	for pq.Len() > 0 {
		cur := heap.Pop(pq).(*node).P
		if visited[cur] {
			continue
		}
		visited[cur] = true
		if cur == goal {
			path, ok = reconstruct(prev, start, goal)
			return path, dist[cur], ok
		}
		for _, nb := range g.Neighbors4(cur) {
			if visited[nb] {
				continue
			}
			nd := dist[cur] + g.CostToEnter(nb)
			if d, ok := dist[nb]; !ok || nd < d {
				dist[nb] = nd
				prev[nb] = cur
				heap.Push(pq, &node{P: nb, Pri: nd})
			}
		}
	}
	return nil, 0, false
}

// ---------- GBFS（贪心最佳优先，只看启发；非最优） ----------
func GBFS(g *Grid, start, goal Point) (path []Point, cost int, ok bool) {
	if !g.Walkable(start) || !g.Walkable(goal) {
		return nil, 0, false
	}
	prev := make(map[Point]Point)
	gCost := map[Point]int{start: 0} // 用来统计真实代价，但不参与优先级
	pq := new(PQ)
	heap.Push(pq, &node{P: start, Pri: Manhattan(start, goal)})

	visited := make(map[Point]bool)
	discovered := make(map[Point]bool)
	discovered[start] = true

	for pq.Len() > 0 {
		cur := heap.Pop(pq).(*node).P
		if visited[cur] {
			continue
		}
		visited[cur] = true
		if cur == goal {
			path, ok = reconstruct(prev, start, goal)
			return path, gCost[cur], ok
		}
		for _, nb := range g.Neighbors4(cur) {
			if visited[nb] || discovered[nb] {
				continue
			}
			discovered[nb] = true
			prev[nb] = cur
			gCost[nb] = gCost[cur] + g.CostToEnter(nb)
			heap.Push(pq, &node{P: nb, Pri: Manhattan(nb, goal)})
		}
	}
	return nil, 0, false
}

// ---------- A*（带权；f=g+h，h=Manhattan*minW） ----------
func AStarWeighted(g *Grid, start, goal Point) (path []Point, cost int, ok bool) {
	if !g.Walkable(start) || !g.Walkable(goal) {
		return nil, 0, false
	}
	// 计算最小权重用于可采纳的启发（h 不高估）
	minW := 1
	foundMin := false
	for y := 0; y < g.H; y++ {
		for x := 0; x < g.W; x++ {
			p := Point{x, y}
			if g.Walkable(p) {
				w := g.CostToEnter(p)
				if !foundMin || w < minW {
					minW = w
					foundMin = true
				}
			}
		}
	}
	if !foundMin {
		return nil, 0, false
	}

	gScore := map[Point]int{start: 0}
	fScore := map[Point]int{start: minW * Manhattan(start, goal)}
	prev := make(map[Point]Point)

	pq := new(PQ)
	heap.Push(pq, &node{P: start, Pri: fScore[start]})

	closed := make(map[Point]bool)

	for pq.Len() > 0 {
		cur := heap.Pop(pq).(*node).P
		if closed[cur] {
			continue
		}
		closed[cur] = true
		if cur == goal {
			path, ok = reconstruct(prev, start, goal)
			return path, gScore[cur], ok
		}
		for _, nb := range g.Neighbors4(cur) {
			if closed[nb] {
				continue
			}
			tentativeG := gScore[cur] + g.CostToEnter(nb)
			if g0, ok := gScore[nb]; !ok || tentativeG < g0 {
				gScore[nb] = tentativeG
				prev[nb] = cur
				fScore[nb] = tentativeG + minW*Manhattan(nb, goal)
				heap.Push(pq, &node{P: nb, Pri: fScore[nb]})
			}
		}
	}
	return nil, 0, false
}

// ---------- 最小堆 ----------
type node struct {
	P   Point
	Pri int // 优先级：越小越优
	i   int // heap 内部索引
}
type PQ []*node

func (h PQ) Len() int            { return len(h) }
func (h PQ) Less(i, j int) bool  { return h[i].Pri < h[j].Pri }
func (h PQ) Swap(i, j int)       { h[i], h[j] = h[j], h[i]; h[i].i = i; h[j].i = j }
func (h *PQ) Push(x any)         { n := x.(*node); n.i = len(*h); *h = append(*h, n) }
func (h *PQ) Pop() any           { old := *h; n := len(old); x := old[n-1]; *h = old[:n-1]; x.i = -1; return x }
func (h *PQ) update(n *node, p int) { n.Pri = p; heap.Fix(h, n.i) }

// ---------- 示例 ----------
func main() {
	g := NewGrid(10, 8)

	// 随便放一些墙
	for x := 3; x <= 3; x++ {
		for y := 1; y <= 6; y++ {
			g.Walls[Point{x, y}] = true
		}
	}
	// 带权（进入该格的代价）：中间一带更重
	for y := 0; y < g.H; y++ {
		for x := 0; x < g.W; x++ {
			p := Point{x, y}
			if g.Walkable(p) && (x >= 5 && x <= 7) {
				g.Weights[p] = 4 // 进入这些格子的代价为 4
			}
		}
	}
	// 其他未设权重的格子默认 1
	start := Point{0, 0}
	goal := Point{9, 7}

	fmt.Println("Start:", start, "Goal:", goal)

	if path, cost, ok := BFS(g, start, goal); ok {
		fmt.Printf("BFS: steps=%d  pathLen=%d\n", cost, len(path))
	} else {
		fmt.Println("BFS: no path")
	}

	if path, cost, ok := Dijkstra(g, start, goal); ok {
		fmt.Printf("Dijkstra: cost=%d  pathLen=%d\n", cost, len(path))
	} else {
		fmt.Println("Dijkstra: no path")
	}

	if path, cost, ok := GBFS(g, start, goal); ok {
		fmt.Printf("GBFS: cost=%d  pathLen=%d (not guaranteed optimal)\n", cost, len(path))
	} else {
		fmt.Println("GBFS: no path")
	}

	if path, cost, ok := AStarWeighted(g, start, goal); ok {
		fmt.Printf("A* (weighted): cost=%d  pathLen=%d\n", cost, len(path))
	} else {
		fmt.Println("A* (weighted): no path")
	}
}

